/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package proyecto1IA;

import java.awt.Point;
import java.util.ArrayList;

/**
 *
 * @author usuario
 */
public class AEstrella {

    private int matrizPesos[][];
    private Point robot, objeto1, objeto2, lugar1, lugar2;
    private boolean encontradoObj1, encontradoObj2, entregadoLugar1, entregadoLugar2;
    public ArrayList<ArrayList<Point>> solucion;

    public AEstrella(int[][] matrizPesos, Point robot, Point objeto1, Point objeto2, Point lugar1, Point lugar2) {
        this.matrizPesos = matrizPesos;
        this.robot = robot;
        this.objeto1 = objeto1;
        this.objeto2 = objeto2;
        this.lugar1 = lugar1;
        this.lugar2 = lugar2;
        encontradoObj1 = false;
        encontradoObj2 = false;
        entregadoLugar1 = false;
        entregadoLugar2 = false;
    }

    public ArrayList<ArrayList<Point>> controladorAEstrella() {
        EstadoAEstrella actual;
        solucion = new ArrayList<ArrayList<Point>>();

        int opcion1 = distanciaManhattan(robot, objeto1);
        int opcion2 = distanciaManhattan(robot, objeto2);
        p("Hasta el objeto 1: " + opcion1);
        p("Hasta el objeto 2: " + opcion2);

        if (opcion1 < opcion2) {
            actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion1);
            ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
            raizRobot.add(actual);
            p("Opcion1 es menor");
            solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, objeto1));
            solucion.get(0).remove(solucion.get(0).size() - 1);
            encontradoObj1 = true;
            robot = objeto1;
        } else {
            actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion2);
            ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
            raizRobot.add(actual);
            p("Opcion2 es menor");
            solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, objeto2));
            solucion.get(0).remove(solucion.get(0).size() - 1);
            encontradoObj2 = true;
            robot = objeto2;
        }
        //luego de haber encontrado el objeto1
        if (encontradoObj1) {
            opcion1 = distanciaManhattan(robot, objeto2);
            opcion2 = distanciaManhattan(robot, lugar1);
            p("Hasta el objeto 2: " + opcion1);
            p("Hasta el lugar 1: " + opcion2);

            if (opcion1 < opcion2) {
                actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion1);
                ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
                raizRobot.add(actual);

                p("Opcion1 es menor");
                solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, objeto2));
                solucion.get(1).remove(solucion.get(1).size() - 1);
                encontradoObj2 = true;
                robot = objeto2;
            } else {
                actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion2);
                ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
                raizRobot.add(actual);

                p("Opcion2 es menor");
                solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, lugar1));
                solucion.get(1).remove(solucion.get(1).size() - 1);
                entregadoLugar1 = true;
                robot = lugar1;
            }

        } //Si recogió el 2 objeto mira si puede entregarlo o ir por el otro objeto 1
        else if (encontradoObj2) {
            opcion1 = distanciaManhattan(robot, objeto1);
            opcion2 = distanciaManhattan(robot, lugar2);
            p("Hasta el objeto 1: " + opcion1);
            p("Hasta el lugar 2: " + opcion2);

            if (opcion1 < opcion2) {
                actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion1);
                ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
                raizRobot.add(actual);

                p("Opcion1 es menor");
                solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, objeto1));
                solucion.get(1).remove(solucion.get(1).size() - 1);
                encontradoObj1 = true;
                robot = objeto1;
            } else {
                actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion2);
                ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
                raizRobot.add(actual);

                p("Opcion2 es menor");
                solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, lugar2));
                solucion.get(1).remove(solucion.get(1).size() - 1);
                entregadoLugar2 = true;
                robot = lugar2;
            }
        }

        //Revisa si encontró un objeto o si lo entregó
        if (encontradoObj1 && entregadoLugar1) {
            opcion1 = distanciaManhattan(robot, objeto2);
            p("Hasta el objeto 2: " + opcion1);

            actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion1);
            ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
            raizRobot.add(actual);

            p("Unica Opcion1 buscar objeto 2 es menor");
            solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, objeto2));
            solucion.get(2).remove(solucion.get(2).size() - 1);
            encontradoObj2 = true;
            robot = objeto2;


        } else if (encontradoObj2 && entregadoLugar2) {
            opcion2 = distanciaManhattan(robot, objeto1);
            p("Hasta el objeto 1: " + opcion2);

            actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion2);
            ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
            raizRobot.add(actual);

            p("Unica Opcion2 buscar objeto 1 es menor");
            solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, objeto1));
            solucion.get(2).remove(solucion.get(2).size() - 1);
            encontradoObj1 = true;
            robot = objeto1;


        } else if (encontradoObj1 && encontradoObj2) {
            opcion1 = distanciaManhattan(robot, lugar1);
            opcion2 = distanciaManhattan(robot, lugar2);

            p("Hasta el lugar 1: " + opcion1);
            p("Hasta el lugar 2: " + opcion2);

            //imprimir("ENcontradosObj1 y ENcontradoObj2");
            //imprimir("lugar 1: " + lugar1 + " lugar 2: "+ lugar2);
            if (opcion1 <= opcion2) {
                actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion1);
                ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
                raizRobot.add(actual);

                p("Opcion1 es menor");
                solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, lugar1));
                solucion.get(2).remove(solucion.get(2).size() - 1);
                entregadoLugar1 = true;
                robot = lugar1;
            } else {
                actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion2);
                ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
                raizRobot.add(actual);

                p("Opcion2 es menor");
                solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, lugar2));
                solucion.get(2).remove(solucion.get(2).size() - 1);
                entregadoLugar2 = true;
                robot = lugar2;
            }
        }

        //Busca la última meta a la que levar el objeto restante
        if (entregadoLugar1) {
            opcion1 = distanciaManhattan(robot, lugar2);
            p("Hasta el lugar 2: " + opcion1);
            //imprimir("EntregadoObj1");
            actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion1);
            ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
            raizRobot.add(actual);

            p("Unica Opcion1 es dejar el objeto 2, es menor");
            solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, lugar2));
            entregadoLugar2 = true;
            robot = lugar2;

        } else if (entregadoLugar2) {
            opcion2 = distanciaManhattan(robot, lugar1);
            p("Hasta el lugar 2: " + opcion2);
            //imprimir("EntregadoObj1");
            actual = new EstadoAEstrella(robot, new Point(-1, -1), 0, opcion2);
            ArrayList<EstadoAEstrella> raizRobot = new ArrayList();
            raizRobot.add(actual);

            p("Unica Opcion1 es dejar el objeto 1, es menor");
            solucion.add(buscarObjetivo(new ArrayList(), raizRobot, new ArrayList(), robot, lugar1));
            entregadoLugar1 = true;
            robot = lugar1;
        }

        return solucion;
    }

    public int distanciaManhattan(Point partida, Point llegada) {
        int distancia = 0;
        if (partida.x > llegada.x) {
            distancia += partida.x - llegada.x;
        } else {
            distancia += llegada.x - partida.x;
        }

        if (partida.y > llegada.y) {
            distancia += partida.y - llegada.y;
        } else {
            distancia += llegada.y - partida.y;
        }

        return distancia;

    }

    public ArrayList<Point> buscarObjetivo(ArrayList<EstadoAEstrella> nodosExpandidos, ArrayList<EstadoAEstrella> nodosARevisar, ArrayList<EstadoAEstrella> nodosRevisados, Point partida, Point objetivo) {
        System.out.println("Peso: " + nodosARevisar.get(0).getPeso());
        EstadoAEstrella actual = new EstadoAEstrella(new Point(-1, -1), new Point(-1, -1), -1, -1);
        int menor = Integer.MAX_VALUE;
        for (int posicion = 0; posicion < nodosARevisar.size(); posicion++) {
            if (!(estaEnLista(nodosExpandidos, nodosARevisar.get(posicion))) || nodosExpandidos.isEmpty()) {
                nodosExpandidos.add(nodosARevisar.get(posicion));
            }

            if (nodosARevisar.get(posicion).getPeso() < menor) {
                menor = nodosARevisar.get(posicion).getPeso();
                actual = nodosARevisar.get(posicion);
            }
        }
        p("");

        p(actual.getPunto() + " y la distancia es: " + actual.getDistanciaAMeta());


        if (partida.x >= objetivo.x) {
            if (partida.y >= objetivo.y) {
                p("partida x y y son mayores");
                if (!(actual.getPunto().equals(objetivo) && actual.getDistanciaAMeta() == 0)) {
                    nodosARevisar.remove(actual);
                    nodosRevisados.add(actual);

                    //Arriba
                    if (actual.getPunto().x - 1 >= 0 && !(actual.getPunto().x - 1 < objetivo.x)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0)) || estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0)))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x - 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Derecha
                    if (actual.getPunto().y + 1 < matrizPesos.length && !(actual.getPunto().y + 1 == partida.y + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0)) || estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0)))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y + 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Abajo
                    if (actual.getPunto().x + 1 < matrizPesos.length && !(actual.getPunto().x + 1 == partida.x + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x + 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Izquierda
                    if (actual.getPunto().y - 1 >= 0 && !(actual.getPunto().y - 1 < objetivo.y)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y - 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    ArrayList<Point> solucionParcial = buscarObjetivo(nodosExpandidos, nodosARevisar, nodosRevisados, partida, objetivo);
                    if (!(solucionParcial.isEmpty())) {
                        return solucionParcial;
                    }

                } else {
                    //Debemos agregar el encontrado a los nodos Expandidos ya que hay que mostrarlos
                    nodosExpandidos.add(actual);
                    p("LLegó al objetivo!");

                    ArrayList<Point> camino = new ArrayList();
                    camino.add(actual.getPunto());

                    while (!(actual.predecesor.equals(new Point(-1, -1)))) {

                        camino.add(0, actual.getPredecesor());

                        for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {

                            if (actual.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                                actual = nodosExpandidos.get(i);
                                break;
                            }
                        }
                    }
                    return camino;
                }
            } else {
                p("partida x y Objetivo y son mayores");
                if (!(actual.getPunto().equals(objetivo) && actual.getDistanciaAMeta() == 0)) {
                    nodosARevisar.remove(actual);
                    nodosRevisados.add(actual);


                    //Arriba
                    if (actual.getPunto().x - 1 >= 0 && !(actual.getPunto().x - 1 < objetivo.x)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x - 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Derecha
                    if (actual.getPunto().y + 1 < matrizPesos.length && !(actual.getPunto().y + 1 == objetivo.y + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y + 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Abajo
                    if (actual.getPunto().x + 1 < matrizPesos.length && !(actual.getPunto().x + 1 == partida.x + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x + 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Izquierda
                    if (actual.getPunto().y - 1 >= 0 && !(actual.getPunto().y - 1 < partida.y)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y - 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    ArrayList<Point> solucionParcial = buscarObjetivo(nodosExpandidos, nodosARevisar, nodosRevisados, partida, objetivo);
                    if (!(solucionParcial.isEmpty())) {
                        return solucionParcial;
                    }

                } else {
                    //Debemos agregar el encontrado a los nodos Expandidos ya que hay que mostrarlos
                    nodosExpandidos.add(actual);
                    p("LLegó al objetivo!");

                    ArrayList<Point> camino = new ArrayList();
                    camino.add(actual.getPunto());

                    while (!(actual.predecesor.equals(new Point(-1, -1)))) {

                        camino.add(0, actual.getPredecesor());

                        for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {

                            if (actual.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                                actual = nodosExpandidos.get(i);
                                break;
                            }
                        }
                    }
                    return camino;
                }
            }
        } else if (partida.x < objetivo.x) {
            p("x es menor");
            if (partida.y >= objetivo.y) {

                if (!(actual.getPunto().equals(objetivo) && actual.getDistanciaAMeta() == 0)) {
                    nodosARevisar.remove(actual);
                    nodosRevisados.add(actual);

                    //Arriba
                    if (actual.getPunto().x - 1 >= 0 && !(actual.getPunto().x - 1 < partida.x)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x - 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Derecha
                    if (actual.getPunto().y + 1 < matrizPesos.length && !(actual.getPunto().y + 1 == partida.y + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y + 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Abajo
                    if (actual.getPunto().x + 1 < matrizPesos.length && !(actual.getPunto().x + 1 == objetivo.x + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x + 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Izquierda
                    if (actual.getPunto().y - 1 >= 0 && !(actual.getPunto().y - 1 < objetivo.y)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y - 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    p("");
                    ArrayList<Point> solucionParcial = buscarObjetivo(nodosExpandidos, nodosARevisar, nodosRevisados, partida, objetivo);
                    if (!(solucionParcial.isEmpty())) {
                        return solucionParcial;
                    }

                } else {
                    //Debemos agregar el encontrado a los nodos Expandidos ya que hay que mostrarlos
                    nodosExpandidos.add(actual);
                    p("LLegó al objetivo!");

                    ArrayList<Point> camino = new ArrayList();
                    camino.add(actual.getPunto());

                    while (!(actual.predecesor.equals(new Point(-1, -1)))) {

                        camino.add(0, actual.getPredecesor());

                        for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {

                            if (actual.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                                actual = nodosExpandidos.get(i);
                                break;
                            }
                        }
                    }
                    return camino;
                }


            } else {
                p("Anmbas son menores");
                if (!(actual.getPunto().equals(objetivo) && actual.getDistanciaAMeta() == 0)) {
                    nodosARevisar.remove(actual);
                    nodosRevisados.add(actual);

                    //Arriba
                    if (actual.getPunto().x - 1 >= 0 && !(actual.getPunto().x - 1 < partida.x)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x - 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x - 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Derecha
                    if (actual.getPunto().y + 1 < matrizPesos.length && !(actual.getPunto().y + 1 == objetivo.y + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y + 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y + 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Abajo
                    if (actual.getPunto().x + 1 < matrizPesos.length && !(actual.getPunto().x + 1 == objetivo.x + 1)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x + 1, actual.getPunto().y), actual.getPunto(), matrizPesos[actual.getPunto().x + 1][actual.getPunto().y] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    //Izquierda
                    if (actual.getPunto().y - 1 >= 0 && !(actual.getPunto().y - 1 < partida.y)) {
                        if (!(estaEnLista(nodosARevisar, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0)) || (estaEnLista(nodosRevisados, new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), new Point(), 0, 0))))) {
                            nodosARevisar.add(new EstadoAEstrella(new Point(actual.getPunto().x, actual.getPunto().y - 1), actual.getPunto(), matrizPesos[actual.getPunto().x][actual.getPunto().y - 1] + actual.getPeso(), actual.getDistanciaAMeta() - 1));
                        }
                    }
                    ArrayList<Point> solucionParcial = buscarObjetivo(nodosExpandidos, nodosARevisar, nodosRevisados, partida, objetivo);
                    if (!(solucionParcial.isEmpty())) {
                        return solucionParcial;
                    }

                } else {
                    //Debemos agregar el encontrado a los nodos Expandidos ya que hay que mostrarlos
                    nodosExpandidos.add(actual);
                    p("LLegó al objetivo!");

                    ArrayList<Point> camino = new ArrayList();
                    camino.add(actual.getPunto());

                    while (!(actual.predecesor.equals(new Point(-1, -1)))) {

                        camino.add(0, actual.getPredecesor());

                        for (int i = nodosExpandidos.size() - 1; i >= 0; i--) {

                            if (actual.getPredecesor().equals(nodosExpandidos.get(i).getPunto())) {
                                actual = nodosExpandidos.get(i);
                                break;
                            }
                        }
                    }
                    return camino;
                }
            }
        } else {
            return new ArrayList();
        }

        return new ArrayList();
    }

    public boolean estaEnLista(ArrayList<EstadoAEstrella> lista, EstadoAEstrella actual) {

        for (int recorrido = 0; recorrido < lista.size(); recorrido++) {
            if (!lista.isEmpty()) {
                if (lista.get(recorrido).getPunto().x == actual.getPunto().x
                        && lista.get(recorrido).getPunto().y == actual.getPunto().y) {
                    return true;
                }
            }
        }

        return false;

    }

    public void p(Object mensaje) {

        System.out.println(mensaje);

    }

    public void ps(Object mensaje) {

        System.out.print(mensaje + " ");

    }
}
